/*
 * @Author: your name
 * @Date: 2021-06-12 07:28:47
 * @LastEditTime: 2021-06-14 06:29:40
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /lab_1_2_geometry_unfinished/intrinsics.cpp
 */
#include "intrinsics.h"

Eigen::Matrix3d Intrinsics::toCalibrationMatrix()
{
  Eigen::Matrix3d  m;
  m<<fu,0,cu,0,fv,cv,0,0,1;
<<<<<<< HEAD
=======
//   std::cout<<m<<std::endl;
>>>>>>> b9a472b81f35879a7cac71db43d47f77b013f5bf
  return m;
}

Intrinsics::Vector5d Intrinsics::toDistortionCoefficientVector()
{
  Vector5d v;
  v<<k1,k2,0.0,0.0,k3;
  return v;
//   return Vector5d(k1,k2,0,0,k3);
}

std::istream& operator>>(std::istream& is, Intrinsics& intrinsics)
{
  is >> intrinsics.fu
     >> intrinsics.fv
     >> intrinsics.s
     >> intrinsics.cu
     >> intrinsics.cv
     >> intrinsics.k1
     >> intrinsics.k2
     >> intrinsics.k3;

  if (!is)
  {
    throw std::runtime_error("Could not read Intrinsics data");
  }

  return is;
}

std::ostream& operator<<(std::ostream& os, const Intrinsics& intrinsics)
{
  os << "fu: " << intrinsics.fu << "\n"
     << "fv: " << intrinsics.fv << "\n"
     << " s: " << intrinsics.s << "\n"
     << "cu: " << intrinsics.cu << "\n"
     << "cv: " << intrinsics.cv << "\n"
     << "k1: " << intrinsics.k1 << "\n"
     << "k2: " << intrinsics.k2 << "\n"
     << "k3: " << intrinsics.k3 << "\n";

  return os;
}
